1. Field of the Disclosure
This invention relates to a method and system for integrating data from different sensors and estimating the position, velocity and orientation of a vehicle. It is particularly suited to being applied to Unmanned Aerial Vehicles (UAV) incorporating low-cost sensors for:                determining the attitude and orientation of the vehicle;        determining its position and velocity;        navigating during limited time periods without GPS.        
The invention is comprised within the fields of sensor fusion, attitude determination and inertial navigation.
2. Description of Related Art
There are several methods for estimating the position, velocity and orientation of a vehicle:
1. Inertial Navigation System or INS: Integrates the angular accelerations and velocities provided by an Inertial Measurement Unit (IMU) to calculate the position, velocity and orientation of the vehicle. Since this integration is neutrally stable, errors accumulate and the obtained solutions quickly diverge unless very high quality sensors are used, increasing system cost and weight.
The solution is to stabilize the integration by means of closed loop feedback with measurements from other sensors that do not diverge over time. Airspeed measurements and the measurements provided by GPS and magnetometers are used for this purpose. Several methods are used to integrate all or part of these measurements:
1.1. Linear Kalman Filter: It is the simplest filter with the lowest computational cost and therefore very interesting for low-cost applications. The drawback is that it is applicable only to linear or linearized dynamical systems. Therefore it can only be used in certain cases.
1.2. Complementary Kalman Filter: INS algorithms are used to integrate the measurements of an IMU that may be a low-cost IMU. The INS inputs are corrected with the outputs of a linear Kalman filter consisting of an INS and measurement error model and fed by the error between the position and velocity estimated by the INS and the measurements by the remaining sensors. The drawback is that linearization leading to the error model means that the global convergence is not assured and spurious updates could lead to system divergence; furthermore, like all Kalman filters its design implies knowledge of statistics of both the measurement noise and process noise and that these noises are white, Gaussian and non-correlated noises, which in the case of low-cost sensors occurs rather infrequently. Its computational cost ranges between moderate to high, depending on the size of the state vectors and measurements.
1.3. Extended Kalman Filter: This is probably the most widely used filter as it is more precise than the standard Kalman filter. It can estimate the vehicle dynamics which is generally not linear because it allows non-linear terms both in the model and in the measurements. It has a higher risk of divergence than the standard Kalman because the covariance equations are based on the linearized system and not on the real non-linear system. Its asymptotic local stability has been proven, but its global stability cannot be assured. In addition to sharing with the remaining Kalman filters the need to know the noise and measurement statistics, its computational cost is high.
2. Static or Single Frame Method: Unlike the previous filtering methods, a static estimation is carried out consisting of obtaining the orientation from a set of unit vector measurements in both body axes and reference axes. Almost all logarithms (Davenport's, QUEST, FOAM, . . .) are based on resolving the Wahba problem which consists of finding an orthogonal matrix with a +1 determinant minimizing a cost function made up of the weighted sum of error squares between the unit vectors in body axes and the result of transforming the vectors into reference axes by the matrix that is sought. It is usually applied in space systems in which the unit vectors are obtained by pointing at the sun and other stars.
There are some examples of the application to a dynamic estimation but it is based on its integration in a linear or complementary Kalman filter with the previously discussed drawbacks.